package es.uji.viselab.robot;

import es.uji.viselab.math.Joints;
import es.uji.viselab.math.Matrix4;
import es.uji.viselab.math.Vector3;

public class SimulatedRobot extends Robot {

	// Pose end effector based on robot coordinate system
	// double[][] pose;

	public SimulatedRobot(RobotCatalog robotModel) {
		super(robotModel);
		this.robotModel = robotModel;

		// Position of the end effector
		Matrix4 orientation = new Matrix4();
		orientation.setIdentity();
		orientation.rotY(Math.PI);
		Matrix4 translation = new Matrix4();
		translation.set(new Vector3(1.0, 0.0, 1.0));
		pose = new Matrix4();
		pose.mul(translation, orientation);

	}

	@Override
	public void connect(String ip, int port) throws RobotException {
		super.connect(ip, port);
		if (connected) {
			throw new RobotException("Cannot connect to robot, it is already connected");
		} else {
			connected = true;
		}
	}

	@Override
	public boolean isConnected() {
		return this.connected;
	}

	@Override
	public String ping() throws RobotException {
		return "Pong from Virtual Robot";
	}

	@Override
	public void disconnect() throws RobotException {
		if (connected) {
			connected = false;
		} else {
			throw new RobotException("Cannot disconnect from robot, I am not connected");
		}
	}

	@Override
	public void move(Joints j) throws RobotException {

		this.joints = j.getArray();
		notifyPositionChange(this);

	}

	@Override
	public boolean isVirtual() {
		return true;
	}

	@Override
	public boolean isRobotConnected() {
		return connected;
	}

	@Override
	public Joints getJoints() throws RobotException {
		double[] j = robotModel.getDefaultJointsPosition();
		if (j.length == 6)
			return new Joints(j[0], j[1], j[2], j[3], j[4], j[5]);
		if (j.length == 0)
			return null;
		return null;
	}

	@Override
	public void move(double[] v) throws RobotException {

		Matrix4 translate = new Matrix4();
		Matrix4 rotateZ = Matrix4.getIdentityM();
		
		translate.set(new Vector3(v[0], v[1], v[2]));
		rotateZ.rotZ(v[5]*Math.PI/360);
		pose.mul(pose, rotateZ);
		pose.mul(translate, pose);
		notifyPositionChange(this);
	}

	@Override
	public void setPosition(double[] v) throws RobotException {

		double[] displacement = new double[6];
		displacement[0] = v[0] - pose.m03;
		displacement[1] = v[1] - pose.m13;
		displacement[2] = v[2] - pose.m23;
		displacement[3] = 0;
		displacement[4] = 0;
		displacement[5] = 0;
		move(displacement);
	}

	@Override
	public String getName() {
		// TODO Auto-generated method stub
		return null;
	}

}
